#include "./handler_task.h"
#include "../pub_driver/cmd_can.h"
#include "../cargo_driver/cargo_motor.h"
#include "../cargo_driver/pick_motor.h"
#include "../utils/log.h"
#include "../cargo_driver/motors.h"
#include "../cargo_driver/scan_uart.h"
#include "../pub_service/ota_service.h"
#include "../cargo_driver/sensor.h"
#include "../cfg/cargo_info.h"

namespace cargo_task {

HandlerTask handlerTask;

using namespace pub_driver;
using namespace cargo_driver;
using namespace conn;
using namespace pub_service;
using namespace cfg;

void HandlerTask::init_start() {
    m_mq.init();
    start();
}

static void on_cargo_ctrl(const Frame& frame) {
    uint8_t row;
    uint16_t offset;
    Error err = frame.parse(row, offset);
    if (err) {
        goto finish;
    }
    err = cargoMotor->run_with_row(row, offset);
finish:
    cmdCAN->fast_res(frame, err);
}

static void on_pick_ctrl(const Frame &frame) {
    uint8_t col;
    uint16_t offset;
    Error err = frame.parse(col, offset);
    if (err) {
        goto finish;
    }
    err = pickMotor.async_run_with_col(col, offset);
    if (err) {
        goto finish;
    }
    err = pickMotor.wait_for();
finish:
    cmdCAN->fast_res(frame, err);
}

static void on_box_motor_ctrl(const Frame &frame) {
    uint8_t dir;
    Error err = frame.parse(dir);
    if (err) {
        goto finish;
    }   
    switch (dir) {
        case 0: err = motors.box_up(); break;
        case 1: err = motors.box_down(); break;
        default: err = ec::pub::INVALID; break;
    }
finish:
    cmdCAN->fast_res(frame, err);
}

static void on_comp_ctrl(const Frame &frame) {
    uint8_t dir;
    Error err = frame.parse(dir);
    if (err) {
        goto finish;
    }   
    switch (dir) {
        case 0: motors.comp_open(); break;
        case 1: motors.comp_close(); break;
        default: err = ec::pub::INVALID; break;
    }
finish:
    cmdCAN->fast_res(frame, err);
}

static void on_scan(const Frame &frame) {
    ByteView bw;
    Error err = scanUart.scan(bw);
    cmdCAN->fast_res(frame, err, bw);
}

static void on_cargo_test(const Frame &frame) {
    uint8_t dir;
    uint16_t freq;
    Error err = frame.parse(dir, freq);

    if ((freq == 0) || (dir > 2)) {
        err = ec::pub::INVALID;
        goto finish;
    }

    switch (dir) {
        case 0: cargoMotor->test_run_cw(freq); break;
        case 1: cargoMotor->test_run_ccw(freq); break;
        case 2: cargoMotor->test_run_stop(); break;
    }

finish:
    cmdCAN->fast_res(frame, err);
}

static void comp_exec() {
    constexpr uint32_t TIMEOUT_COUNT = 2000;
    uint32_t time_count = 0;
    for (;;) {
        if (sensor->is_comp_checked()) {
            break;
        }
        osDelay(200);
        time_count += 200;
        if (time_count >= TIMEOUT_COUNT) {
            break;
        }
    }
}

static void on_pick(const Frame &frame) {
    uint8_t row, col;
    Error err = frame.parse(row, col);
    if (err) {
        goto finish;
    }

    if ((row < 1) || (col < 1)) {
        err = ec::pub::INVALID;
        goto finish;
    }

    err = motors->box_up();
    if (err) {
        goto finish;
    }
    
    err = pickMotor.async_run_with_col(col, cargoInfo->pick_offset);
    if (err) {
        goto finish;
    }

    err = cargoMotor.run_with_row(row, cargoInfo->cargo_pick_offset);
    if (err) {
        goto finish;
    }

    err = pickMotor.wait_for();
    if (err) {
        goto finish;
    }

    motors.comp_open();

    err = motors.box_down();
    if (err) {
        motors.comp_close();
        goto finish;
    }

    comp_exec();

    err = motors.box_up();
    if (err) {
        motors.comp_close();
        goto finish;
    }

    if (!sensor->is_comp_checked()) {
        motors.comp_close();
        err = ec::cargo::COMPR_FAIL;
        goto finish;
    }

finish:
  //  motors.comp_close();

    cmdCAN->fast_res(frame, err);
}

static void do_run_next_row() {
    static uint8_t row = 1;
    if (!sensor->is_btn_click()) {
        return;
    }
    if (row > 28) {
        row = 1;
    }
    cargoMotor->run_with_row(row, 0);
    row += 1;
}

static void on_cargo_set(const Frame &frame) {
    uint16_t cargo, pick;
    Error err = frame.parse(cargo, pick);
    if (err) {
        goto finish;
    }
    cargoInfo->cargo_pick_offset = cargo;
    cargoInfo->pick_offset = pick;
    err = cargoInfo.save();
finish:
    cmdCAN.fast_res(frame, err);
}

void HandlerTask::run() {
    Frame frame;
    for (;;) {

        if (osOK != m_mq.poll(frame, 200)) {
            do_run_next_row();
            continue;
        }

        task_info.seq = frame.seq_cmd();
        task_info.cmd = frame.cmd();

        switch (frame.cmd()) {
            case proto::cargo::CARGO_CTRL: on_cargo_ctrl(frame); break;
            case proto::cargo::PICK_CTRL: on_pick_ctrl(frame); break;
            case proto::cargo::BOX_MOTOR_CTRL: on_box_motor_ctrl(frame); break;
            case proto::cargo::COMP_CTRL: on_comp_ctrl(frame); break;
            case proto::cargo::SCAN: on_scan(frame); break;
            case proto::pub::OTA_START: pub_service::ota_start(frame); break;
            case proto::pub::OTA_TRANSLATE: pub_service::ota_translate(frame); break;
            case proto::pub::OTA_COMPLETE: pub_service::ota_complete(frame); break;
            case proto::cargo::CARGO_TEST: on_cargo_test(frame); break;
            case proto::cargo::CARGO_PICK: on_pick(frame); break;
            case proto::cargo::CARGO_SET_ARG: on_cargo_set(frame); break;
        }
    
        task_info.reset();
    }
}

}

